import matplotlib.pyplot as plt
import numpy as np


def plot_current_best_path(path, distance):
    # 初始化城市和物流中心的坐标
    points_position = [[12.8, 8.5], [18.4, 3.4], [15.4, 16.6], [18.9, 15.2], [15.5, 11.6], [3.9, 10.6], [10.6, 7.6], [8.6, 8.4], [12.5, 2.1], [
        13.8, 5.2], [6.7, 16.9], [14.8, 2.6], [1.8, 8.7], [17.1, 11], [7.4, 1],  [0.2, 2.8], [11.9, 19.8], [13.2, 15.1], [6.4, 5.6], [9.6, 14.8]]
    logistics_center_position = [14.2, 13.1]
    points_position = np.array(points_position)
    # 初始化线的颜色
    line_color = ['-b', '-r', '-g', '-y', '-k']

    # 创建画布
    fig = plt.figure(1)
    # 描点（城市和物流中心）
    for i in range(points_position.shape[0]):
        plt.plot(points_position[i][0], points_position[i][1], '+')
        plt.text(points_position[i][0], points_position[i][1], i+1)
    plt.plot(
        logistics_center_position[0], logistics_center_position[1], 'r*', markersize=15)
    plt.text(logistics_center_position[0],
             logistics_center_position[1], 'center')

    car_path_x = []
    car_path_y = []
    cars_path_x = []
    cars_path_y = []
    # 切分目前得到的最优路径便于划线
    for i in range(len(path) + 1):
        if i == len(path):
            car_path_x.append(logistics_center_position[0])
            car_path_y.append(logistics_center_position[1])
            cars_path_x.append(car_path_x)
            cars_path_y.append(car_path_y)
        elif i == 0 and path[i] == 'new car':
            car_path_x.append(logistics_center_position[0])
            car_path_y.append(logistics_center_position[1])
        elif i != 0 and path[i] == 'new car':
            car_path_x.append(logistics_center_position[0])
            car_path_y.append(logistics_center_position[1])
            cars_path_x.append(car_path_x)
            cars_path_y.append(car_path_y)
            car_path_x = [logistics_center_position[0]]
            car_path_y = [logistics_center_position[1]]
        else:
            car_path_x.append(points_position[path[i] - 1][0])
            car_path_y.append(points_position[path[i] - 1][1])

    i = 0
    # 画线
    for x, y in zip(cars_path_x, cars_path_y):
        plt.plot(x, y, line_color[i])
        i += 1
    # 添加标题
    plt.title('The path distance is {0}'.format(distance))
    # 添加 x 轴和 y 轴的标签
    plt.xlabel('x')
    plt.ylabel('y')
    # 绘图
    plt.show()


if __name__ == "__main__":
    path = ['new car', 6, 13, 16, 15, 19, 8, 1, 'new car', 7, 9, 12,
            14, 4, 'new car', 18, 17, 11, 20, 10, 2, 5, 'new car', 3]
    plot_current_best_path(path, 0)
